#include "LaneSensorPoller.hpp"
#include "TurnOverOnColorLaneState.hpp"
#include "FollowColorLaneState.hpp"
#include "Hardware.hpp"
#include "Robot.hpp"

TurnOverOnColorLaneState::TurnOverOnColorLaneState() : enteringColorLane(false), leftTurn(true) { }

void TurnOverOnColorLaneState::startState()
{
	LaneSensorPoller& poller(LaneSensorPoller::getInstance());
	if (poller.getColorLaneSensorStatus() == RIGHTON)
		leftTurn = 0;
	startManeuvre();
}

void TurnOverOnColorLaneState::startManeuvre()
{
	Robot::getInstance().getEngine().driveBackward();
	Hardware::getHardwareController().wait(500);
	if (leftTurn) Robot::getInstance().getEngine().turnLeft();
	else Robot::getInstance().getEngine().turnRight();
}

void TurnOverOnColorLaneState::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case LANESTATUSCHANGED:
			handleColorLaneEvent((LaneEvent&) event);
		break;
		//TODO: handle collision here
		default:
		break;
	}
}



void TurnOverOnColorLaneState::handleColorLaneEvent(LaneEvent& clEvent)
{
	if (leftTurn)
	{	// Left turn
		if (!enteringColorLane && SensorCheck::leftOn(clEvent.oldColorLaneStatus) && SensorCheck::leftOff(clEvent.newColorLaneStatus))
			enteringColorLane = true;
		else if (enteringColorLane && SensorCheck::leftOff(clEvent.oldColorLaneStatus) && SensorCheck::leftOn(clEvent.newColorLaneStatus))
			Robot::getInstance().changeState(new FollowColorLaneState());
	}
	else
	{	// Right turn
		if (!enteringColorLane && SensorCheck::rightOn(clEvent.oldColorLaneStatus) && SensorCheck::rightOff(clEvent.newColorLaneStatus))
			enteringColorLane = true;
		else if (enteringColorLane && SensorCheck::rightOff(clEvent.oldColorLaneStatus) && SensorCheck::rightOn(clEvent.newColorLaneStatus))
			Robot::getInstance().changeState(new FollowColorLaneState());
	}
}

State::StateType TurnOverOnColorLaneState::getStateType()
{
	return State::TURNOVERONCOLORLANE;
}
